

#include <QDebug>

#include "grtmatrix.h"


// ===========================================================================
//                                  GRTMatrix
// ===========================================================================


// -------------------------- GRTMatrix::GRTMatrix() -------------------------
GRTMatrix::GRTMatrix()
{	
	_unit = RADIANS;
	reset();
}
// ---------------------------------------------------------------------------


// -------------------- GRTMatrix::GRTMatrix() -------------------------------
GRTMatrix::GRTMatrix( const GRTMatrix & matrix )
{
	setMatrix( matrix );
}
// ---------------------------------------------------------------------------


// ---------------------- GRTMatrix::setMatrix() -----------------------------
void GRTMatrix::setMatrix( const GRTMatrix & matrix )
{
	for( int i = 1; i < 5; i++ )
	{
		for( int j = 1; j < 5; j++ )
		{
			setM( i, j, matrix.m( i, j ) );
		}
	}
	setUnit( matrix.unit() );
}
// ---------------------------------------------------------------------------


// ------------------------- GRTMatrix::reset() ------------------------------
void GRTMatrix::reset()
{
		// clear matrix 
	for( int i = 0; i < 16; i++ )
	{
		*(*_matrix + i) = 0;
	}
		// set 1 on the diagonal
	_matrix[0][0] = 1;
	_matrix[1][1] = 1;
	_matrix[2][2] = 1;
	_matrix[3][3] = 1;
}
// ---------------------------------------------------------------------------


// --------------------- GRTMatrix::operator = () ----------------------------
GRTMatrix GRTMatrix::operator = ( const GRTMatrix matrix )
{
	for( int j = 1; j < 5; j++ )
	{
		for( int i = 1; i < 5; i++ )
		{
			setM( i, j, matrix.m( i, j ) );
		}
	}
	
	setUnit( matrix.unit() );
	
	return *this;
}
// ---------------------------------------------------------------------------


// --------------------- GRTMatrix::operator * () ----------------------------
GRTMatrix GRTMatrix::operator * ( const GRTMatrix matrix ) const
{
	GRTMatrix tmp = *this;
	GRTMatrix tmp2;
	
	for( int j = 1; j < 5; j++ )
	{
		for( int i = 1; i < 5; i++ )
		{
			double value = tmp.m( i, 1 ) * matrix.m( 1, j ) +
						tmp.m( i, 2 ) * matrix.m( 2, j ) +
						tmp.m( i, 3 ) * matrix.m( 3, j ) +
						tmp.m( i, 4 ) * matrix.m( 4, j );
			tmp2.setM( i, j, value );
		}
	}
	tmp2.setUnit( tmp.unit() );
	
	return tmp2;
}
// ---------------------------------------------------------------------------


// --------------------- GRTMatrix::operator + () ----------------------------
GRTMatrix GRTMatrix::operator + ( const GRTMatrix matrix ) const
{
	GRTMatrix tmp = *this;
	
	for( int j = 1; j < 5; j++ )
	{
		for( int i = 1; i < 5; i++ )
		{
			double value = tmp.m( i, j ) + matrix.m( i, j );
			tmp.setM( i, j, value );
		}
	}
	
	return tmp;
}
// ---------------------------------------------------------------------------


// --------------------- GRTMatrix::operator - () ----------------------------
GRTMatrix GRTMatrix::operator - ( const GRTMatrix matrix ) const
{
	GRTMatrix tmp = *this;
	
	for( int j = 1; j < 5; j++ )
	{
		for( int i = 1; i < 5; i++ )
		{
			double value = tmp.m( i, j ) - matrix.m( i, j );
			tmp.setM( i, j, value );
		}
	}
	
	return tmp;
}
// ---------------------------------------------------------------------------


// ------------------------ GRTMatrix::m() -----------------------------------
double GRTMatrix::m( int indexX, int indexY ) const
{
	// TODO check if index in range 
	
	return _matrix[indexX-1][indexY-1];
}
// ---------------------------------------------------------------------------


// --------------------------- GRTMatrix::setM() -----------------------------
void GRTMatrix::setM( int indexX, int indexY, double value )
{
	// TODO check if valid index 
	
	_matrix[indexX-1][indexY-1] = value;
}
// ---------------------------------------------------------------------------


// ---------------------- GRTMatrix::translateX() ----------------------------
void GRTMatrix::translateX( const double dx )
{
	GRTMatrix transX;
	transX.setM( 1, 4, dx );
	
	*this = transX * (*this);
}
// ---------------------------------------------------------------------------


// ---------------------- GRTMatrix::translateY() ----------------------------
void GRTMatrix::translateY( const double dy )
{
	GRTMatrix transY;
	transY.setM( 2, 4, dy );
	
	*this = transY * (*this);
}
// ---------------------------------------------------------------------------


// ---------------------- GRTMatrix::translateZ() ----------------------------
void GRTMatrix::translateZ( const double dz )
{
	GRTMatrix transZ;
	transZ.setM( 3, 4, dz );
	
	*this = transZ * (*this);
}
// ---------------------------------------------------------------------------


// ------------------------- GRTMatrix::angle() ------------------------------
double GRTMatrix::angle( const double ang )
{
	switch( _unit )
	{
		case RADIANS:
			return ang;
		
		case DEGREES:
			return ( ang * M_PI / 180 );
		
		default:
			return ang;
	}
}
// ---------------------------------------------------------------------------


// ------------------------ GRTMatrix::rotateX() -----------------------------
void GRTMatrix::rotateX( const double alpha )
{
	double ang = angle( alpha );
	
	GRTMatrix rot;
	
	rot.setM( 2, 2,  cos( ang ) );
	rot.setM( 3, 3,  cos( ang ) );
	rot.setM( 2, 3, -sin( ang ) );
	rot.setM( 3, 2,  sin( ang ) );
	
	*this = rot * (*this);
}
// ---------------------------------------------------------------------------


// ------------------------ GRTMatrix::rotateY() -----------------------------
void GRTMatrix::rotateY( const double beta )
{
	double ang = angle( beta );
	
	GRTMatrix rot;
	
	rot.setM( 1, 1,  cos( ang ) );
	rot.setM( 3, 3,  cos( ang ) );
	rot.setM( 1, 3,  sin( ang ) );
	rot.setM( 3, 1, -sin( ang ) );
	
	*this = rot * (*this);
}
// ---------------------------------------------------------------------------


// ------------------------ GRTMatrix::rotateZ() -----------------------------
void GRTMatrix::rotateZ( const double beta )
{
	double ang = angle( beta );
	
	GRTMatrix rot;
	
	rot.setM( 1, 1,  cos( ang ) );
	rot.setM( 2, 2,  cos( ang ) );
	rot.setM( 1, 2, -sin( ang ) );
	rot.setM( 2, 1,  sin( ang ) );
	
	*this = rot * (*this);
}
// ---------------------------------------------------------------------------


// -------------------------- GRTMatrix::toString() --------------------------
QString GRTMatrix::toString()
{
	QString line( "\n" );
	
	for( int i = 1; i < 5; i++ )
	{
		line.append( QString( "%1 %2 %3 %4\n" )
				.arg( m( i, 1 ), 7, 'f', 2 )
				.arg( m( i, 2 ), 7, 'f', 2 )
				.arg( m( i, 3 ), 7, 'f', 2 )
				.arg( m( i, 4 ), 7, 'f', 2 ) );
	}
	
	return line;
}
// ---------------------------------------------------------------------------


// eof
